#ifndef __BBHAND_CHANDBOX_H_
#define __BBHAND_CHANDBOX_H_

#include <robot/robot.h>
#include <robot/sdh/enums.h>
#include <blackbox/bbox.h>
#include <blackbox/cdatain.h>
#include <blackbox/cdataout.h>
#include <blackbox/carrayin.h>
#include <blackbox/carrayout.h>
#include <blackbox/cmethod.h>

#include <buola/io/csocketacceptor.h>
#include <buola/xml.h>

#include <sdh/sdh.h>

namespace buola { namespace robot { namespace sdh {

using namespace buola;

class BHand : public blackbox::BBox
{
public:
    BHand();

private:
    void InitHand();
    
    void OnPullJoints();
    void OnPullStatus();
    
    void SetVelocity(const mat::CVecNd<7> &pJoints){}
    void SetGoal(const mat::CVecNd<7> &pJoints);
    void SetCurrent(const mat::CVecNd<7> &pJoints);
    void SetJointVelocity(int pJoint,double pVelocity){}
    void SetJointGoal(int pJoint,double pGoal){}
    
public:
    blackbox::CArrayOut<double,blackbox::NPullOutput> oJoints;
    blackbox::CDataOut<int,blackbox::NPullOutput> oStatus;

    blackbox::CMethod<void(mat::CVecNd<7>)> mSetVelocity;
    blackbox::CMethod<void(mat::CVecNd<7>)> mSetGoal;
    blackbox::CMethod<void(mat::CVecNd<7>)> mSetCurrent;

    blackbox::CMethod<void(int,double)> mSetJointVelocity;
    blackbox::CMethod<void(int,double)> mSetJointGoal;
    
private:
    std::vector<int> mAxis;
    std::vector<double> mActual;
    
    std::unique_ptr<cSDH> mSDH;

    std::mutex mMutex;
};

/*namespace sdh*/ } /*namespace robot*/ } /*namespace buola*/ }

#endif
